﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;
using Unity.Mathematics;
using UnityEditor;
using UnityEngine;
using UnityEngine.XR;

namespace lights
{
    
    public struct fp32
    {
        public int value;
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public fp32(int v)
        {
            this.value = v;
        }

        #region 常量
        public const int Shift = 16;
        public const int Scalar = (1 << Shift);

        private const int FractionMask = (1 << Shift) - 1;

        public const int NUM_BITS = 32;

        public static readonly fp32 half = new fp32(Scalar / 2);
        public static readonly fp32 zero = new fp32(0);
        public static readonly fp32 one = new fp32(Scalar);
        public static readonly fp32 onehundred = new fp32(Scalar * 100);

        public static readonly fp32 PI = new fp32((int)(13493037705L >> 16)); //(int)(Math.PI * 65536.0) << 16;
        public static readonly fp32 PI2 = new fp32((int)(26986075409L >> 16));
        public static readonly fp32 PIHalf = new fp32((int)(6746518852L >> 16));
        public static readonly fp32 PI3_4 = PIHalf * fp32.FromInt(3);
        public static readonly fp32 E = new fp32((int)(11674931555L >> 16));
        #endregion

        #region 类型转换
        //作为浮点数显示
        public override string ToString()
        {
            return ToFloat().ToString();
        }
        //精确表达
        public string ToStringFix()
        {
            var h = value / Scalar;
            var l = value % Scalar;
            return h + "+" + l + "/" + Scalar;
        }





        public const int MinValue = -2147483648;
        public static fp32 FromFloat(float v)
        {
            return new fp32((int)(v * Scalar));
        }
        public static fp32 FromInt(int v)
        {
            return new fp32(v * Scalar);
        }
        public float ToFloat()
        {
            return (float)value / (float)Scalar;
        }
        public int ToInt()
        {
            return value >> Shift;
        }
        #endregion
        #region math计算

        //实现一个向上取整
        public static fp32 Ceiling(fp32 v)
        {
            int intValue = v.value >> Shift;
            if ((v.value & FractionMask) != 0)
            {
                intValue++;
            }
            return new fp32(intValue << Shift);
        }

        public static fp32 Floor(fp32 v)
        {
            return new fp32((v.value >> Shift) << Shift);
        }
        public static fp32 Abs(fp32 v)
        {
            return (v.value >= 0) ? v : new fp32(-v.value);
        }
        public static fp32 AbsRef(ref fp32 v)
        {
            return (v.value >= 0) ? v : new fp32(-v.value);
        }
        public static fp32 Sqrt(fp32 x)
        {
            var xl = x.value;
            if (xl < 0)
            {
                // We cannot represent infinities like Single and Double, and Sqrt is
                // mathematically undefined for x < 0. So we just throw an exception.
                throw new ArgumentOutOfRangeException("Negative value passed to Sqrt", "x");
            }

            var num = (ulong)xl;
            var result = 0UL;

            // second-to-top bit
            var bit = 1UL << (NUM_BITS - 2);

            while (bit > num)
            {
                bit >>= 2;
            }

            // The main part is executed twice, in order to avoid
            // using 128 bit values in computations.
            for (var i = 0; i < 2; ++i)
            {
                // First we get the top 48 bits of the answer.
                while (bit != 0)
                {
                    if (num >= result + bit)
                    {
                        num -= result + bit;
                        result = (result >> 1) + bit;
                    }
                    else
                    {
                        result = result >> 1;
                    }
                    bit >>= 2;
                }

                if (i == 0)
                {
                    // Then process it again to get the lowest 16 bits.
                    if (num > (1UL << (NUM_BITS / 2)) - 1)
                    {
                        // The remainder 'num' is too large to be shifted left
                        // by 32, so we have to add 1 to result manually and
                        // adjust 'num' accordingly.
                        // num = a - (result + 0.5)^2
                        //       = num + result^2 - (result + 0.5)^2
                        //       = num - result - 0.5
                        num -= result;
                        num = (num << (NUM_BITS / 2)) - 0x80000000UL;
                        result = (result << (NUM_BITS / 2)) + 0x80000000UL;
                    }
                    else
                    {
                        num <<= (NUM_BITS / 2);
                        result <<= (NUM_BITS / 2);
                    }

                    bit = 1UL << (NUM_BITS / 2 - 2);
                }
            }
            // Finally, if next bit would have been 1, round the result upwards.
            if (num > result)
            {
                ++result;
            }
            return new fp32((int)result);
        }



        public static fp32 SqrtFast(fp32 x)
        {
            // Return 0 for all non-positive values.
            if (x.value <= 0)
            {
                if (x.value < 0)
                    throw new ArgumentOutOfRangeException("Negative value passed to Sqrt", "x"); return fp32.zero;
            }

            // Constants (s2.30).
            const int ONE = (1 << 30);
            const int SQRT2 = 1518500249; // sqrt(2.0)

            // Normalize input into [1.0, 2.0( range (as s2.30).
            int offset = 15 - FPUtil.Nlz((uint)x.value);
            int n = FPUtil.ShiftRight(x.value, offset - 14);
            //Debug.Assert(n >= ONE);
            int y = FPUtil.SqrtPoly4(n - ONE);

            // Divide offset by 2 (to get sqrt), compute adjust value for odd exponents.
            int adjust = ((offset & 1) != 0) ? SQRT2 : ONE;
            offset = offset >> 1;

            // Apply exponent, convert back to s16.16.
            int yr = FPUtil.Qmul30(adjust, y);
            return new fp32(FPUtil.ShiftRight(yr, 14 - offset));
        }
        public static fp32 SqrtFast2(fp32 x)
        {
            // Return 0 for all non-positive values.
            if (x.value <= 0)
            {
                if (x.value < 0)
                    throw new ArgumentOutOfRangeException("Negative value passed to Sqrt", "x");
                return fp32.zero;
            }

            // Constants (s2.30).
            const int ONE = (1 << 30);
            const int SQRT2 = 1518500249; // sqrt(2.0)

            // Normalize input into [1.0, 2.0( range (as s2.30).
            int offset = 15 - FPUtil.Nlz((uint)x.value);
            int n = FPUtil.ShiftRight(x.value, offset - 14);
            //Debug.Assert(n >= ONE);
            int y = FPUtil.SqrtPoly3(n - ONE);

            // Divide offset by 2 (to get sqrt), compute adjust value for odd exponents.
            int adjust = ((offset & 1) != 0) ? SQRT2 : ONE;
            offset = offset >> 1;

            // Apply exponent, convert back to s16.16.
            int yr = FPUtil.Qmul30(adjust, y);
            return new fp32(FPUtil.ShiftRight(yr, 14 - offset));
        }
        #endregion

        #region 四则运算
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 operator +(fp32 lhs, fp32 rhs) { return new fp32(lhs.value + rhs.value); }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 operator -(fp32 lhs, fp32 rhs) { return new fp32(lhs.value - rhs.value); }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 operator *(fp32 lhs, fp32 rhs)
        {
            return new fp32((int)(((long)lhs.value * (long)rhs.value) >> Shift));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 operator /(fp32 lhs, fp32 rhs)
        {

            if (rhs.value == MinValue || rhs.value == 0)
            {
                throw new System.Exception("Fixed32.Div zero error.");

            }

            return new fp32((int)(((long)lhs.value << Shift) / rhs.value));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 operator %(fp32 lhs, fp32 rhs)
        {

            if (rhs.value == 0)
            {
                throw new System.Exception("Fixed32.Mod zero error.");

            }

            return new fp32(lhs.value % rhs.value);
        }

        public static fp32 operator -(fp32 a)
        {
            return new fp32(-a.value);
        }

        #endregion
        ///Compare 方法
        #region Compare方法
        public override bool Equals(object other)
        {
            if (other is fp32 v)
            {
                return this.value == v.value;
            }
            else
            {
                return false;
            }
        }
        public override int GetHashCode()
        {
            return value;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator <(fp32 lhs, fp32 rhs) { return lhs.value < rhs.value; }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator >(fp32 lhs, fp32 rhs) { return lhs.value > rhs.value; }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator <=(fp32 lhs, fp32 rhs) { return lhs.value <= rhs.value; }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator >=(fp32 lhs, fp32 rhs) { return lhs.value >= rhs.value; }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(fp32 lhs, fp32 rhs) { return lhs.value == rhs.value; }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(fp32 lhs, fp32 rhs) { return lhs.value != rhs.value; }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 Min(fp32 x, fp32 y)
        {
            return x.value < y.value ? x : y;
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 Max(fp32 x, fp32 y)
        {
            return x.value > y.value ? x : y;
        }
        #endregion

        #region 三角函数
        private const int RCP_TWO_PI = 683565276;
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 Sin(fp32 v)
        {
            int z = (int)(((long)v.value * (long)RCP_TWO_PI) >> Shift);
            // See: http://www.coranac.com/2009/07/sines/

            // Handle quadrants 1 and 2 by mirroring the [1, 3] range to [-1, 1] (by calculating 2 - z).
            // The if condition uses the fact that for the quadrants of interest are 0b01 and 0b10 (top two bits are different).
            if ((z ^ (z << 1)) < 0)
                z = (1 << 31) - z;

            // Now z is in range [-1, 1].
            const int ONE = (1 << 30);
            // Debug.Assert((z >= -ONE) && (z <= ONE));

            // Polynomial approximation.
            int zz = FPUtil.Qmul30(z, z);
            int res = FPUtil.Qmul30(FPUtil.SinPoly4(zz), z);

            // Return as s2.30.
            return new fp32(res >> 14);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 SinFast(fp32 v)
        {
            int z = (int)(((long)v.value * (long)RCP_TWO_PI) >> Shift);
            // See: http://www.coranac.com/2009/07/sines/

            // Handle quadrants 1 and 2 by mirroring the [1, 3] range to [-1, 1] (by calculating 2 - z).
            // The if condition uses the fact that for the quadrants of interest are 0b01 and 0b10 (top two bits are different).
            if ((z ^ (z << 1)) < 0)
                z = (1 << 31) - z;

            // Now z is in range [-1, 1].
            const int ONE = (1 << 30);
            //Debug.Assert((z >= -ONE) && (z <= ONE));

            // Polynomial approximation.
            int zz = FPUtil.Qmul30(z, z);
            int res = FPUtil.Qmul30(FPUtil.SinPoly3(zz), z);

            // Return as s2.30.
            return new fp32(res >> 14);
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 SinFast2(fp32 v)
        {
            int z = (int)(((long)v.value * (long)RCP_TWO_PI) >> Shift);
            // See: http://www.coranac.com/2009/07/sines/

            // Handle quadrants 1 and 2 by mirroring the [1, 3] range to [-1, 1] (by calculating 2 - z).
            // The if condition uses the fact that for the quadrants of interest are 0b01 and 0b10 (top two bits are different).
            if ((z ^ (z << 1)) < 0)
                z = (1 << 31) - z;

            // Now z is in range [-1, 1].
            const int ONE = (1 << 30);
            //Debug.Assert((z >= -ONE) && (z <= ONE));

            // Polynomial approximation.
            int zz = FPUtil.Qmul30(z, z);
            int res = FPUtil.Qmul30(FPUtil.SinPoly2(zz), z);

            // Return as s2.30.
            return new fp32(res >> 14);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 Cos(fp32 x)
        {
            return Sin(x + PIHalf);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 CosFast(fp32 x)
        {
            return SinFast(x + PIHalf);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 CosFast2(fp32 x)
        {
            return SinFast2(x + PIHalf);
        }



        public static fp32 Acos(fp32 x)
        {
            // Return 0 for invalid values
            if (x.value < -Scalar || x.value > Scalar)
            {
                throw new Exception("Fixed32.Acos out of range.");
            }

            // Compute Atan2(Sqrt((1+x) * (1-x)), x), using s32.32.
            var left = new fp32(Scalar + x.value);
            var right = new fp32(Scalar - x.value);
            var xx = left * right;
            var y = fp32.Sqrt(xx);
            //long xx = (long)(one + x) * (long)(one - x);
            //long y = Fixed64.Sqrt(xx);
            //return (int)(Fixed64.Atan2(y, (long)x << 16) >> 16);
            return fp32.Atan2(y, x);
        }
        public static fp32 AcosFast(fp32 x)
        {
            // Return 0 for invalid values
            if (x.value < -Scalar || x.value > Scalar)
            {
                throw new Exception("Fixed32.Acos out of range.");
            }

            // Compute Atan2(Sqrt((1+x) * (1-x)), x), using s32.32.
            var left = new fp32(Scalar + x.value);
            var right = new fp32(Scalar - x.value);
            var xx = left * right;
            var y = fp32.Sqrt(xx);
            //long xx = (long)(one + x) * (long)(one - x);
            //long y = Fixed64.Sqrt(xx);
            //return (int)(Fixed64.Atan2(y, (long)x << 16) >> 16);
            return fp32.Atan2Fast(y, x);
        }
        public static fp32 AcosFast2(fp32 x)
        {
            // Return 0 for invalid values
            if (x.value < -Scalar || x.value > Scalar)
            {
                throw new Exception("Fixed32.Acos out of range.");
            }

            // Compute Atan2(Sqrt((1+x) * (1-x)), x), using s32.32.
            var left = new fp32(Scalar + x.value);
            var right = new fp32(Scalar - x.value);
            var xx = left * right;
            var y = fp32.Sqrt(xx);
            //long xx = (long)(one + x) * (long)(one - x);
            //long y = Fixed64.Sqrt(xx);
            //return (int)(Fixed64.Atan2(y, (long)x << 16) >> 16);
            return fp32.Atan2Fast2(y, x);
        }



        private static int Atan2Div(int y, int x)
        {
            Debug.Assert(y >= 0 && x > 0 && x >= y);

            // Normalize input into [1.0, 2.0( range (convert to s2.30).
            const int ONE = (1 << 30);
            const int HALF = (1 << 29);
            int offset = 1 - FPUtil.Nlz((uint)x);
            int n = FPUtil.ShiftRight(x, offset);
            Debug.Assert(n >= ONE);

            // Polynomial approximation of reciprocal.
            int oox = FPUtil.RcpPoly4Lut8(n - ONE);
            Debug.Assert(oox >= HALF && oox <= ONE);

            // Apply exponent and multiply.
            int yr = FPUtil.ShiftRight(y, offset);
            return FPUtil.Qmul30(yr, oox);
        }

        private static int Atan2DivFast(int y, int x)
        {
            Debug.Assert(y >= 0 && x > 0 && x >= y);

            // Normalize input into [1.0, 2.0( range (convert to s2.30).
            const int ONE = (1 << 30);
            const int HALF = (1 << 29);
            int offset = 1 - FPUtil.Nlz((uint)x);
            int n = FPUtil.ShiftRight(x, offset);

            // Polynomial approximation.
            int oox = FPUtil.RcpPoly6(n - ONE);
            Debug.Assert(oox >= HALF && oox <= ONE);

            // Apply exponent and multiply.
            int yr = FPUtil.ShiftRight(y, offset);
            return FPUtil.Qmul30(yr, oox);
        }
        private static int Atan2DivFast2(int y, int x)
        {
            Debug.Assert(y >= 0 && x > 0 && x >= y);

            // Normalize input into [1.0, 2.0( range (convert to s2.30).
            const int ONE = (1 << 30);
            const int HALF = (1 << 29);
            int offset = 1 - FPUtil.Nlz((uint)x);
            int n = FPUtil.ShiftRight(x, offset);

            // Polynomial approximation.
            int oox = FPUtil.RcpPoly4(n - ONE);
            Debug.Assert(oox >= HALF && oox <= ONE);

            // Apply exponent and multiply.
            int yr = FPUtil.ShiftRight(y, offset);
            return FPUtil.Qmul30(yr, oox);
        }

        public static fp32 Atan2(fp32 y, fp32 x)
        {
            if (x.value == 0)
            {
                if (y.value > 0) return PIHalf;
                if (y.value < 0) return -PIHalf;

                throw new Exception("Fixed32.Atan2 out of range.");

            }

            var nx = Abs(x);
            var ny = Abs(y);
            int negMask = ((x.value ^ y.value) >> 31);

            if (nx >= ny)
            {
                int k = Atan2Div(ny.value, nx.value);
                int z = FPUtil.AtanPoly5Lut8(k);
                var angle = new fp32((negMask ^ (z >> 14)) - negMask);
                if (x.value > 0) return angle;
                if (y.value >= 0) return angle + PI;
                return angle - PI;
            }
            else
            {
                int k = Atan2Div(nx.value, ny.value);
                int z = FPUtil.AtanPoly5Lut8(k);
                var angle = new fp32(negMask ^ (z >> 14));
                return ((y.value > 0) ? PIHalf : -PIHalf) - angle;
            }
        }
        public static fp32 Atan2Fast(fp32 y, fp32 x)
        {
            if (x.value == 0)
            {
                if (y.value > 0) return PIHalf;
                if (y.value < 0) return -PIHalf;

                throw new Exception("Fixed32.Atan2 out of range.");

            }

            var nx = Abs(x);
            var ny = Abs(y);
            int negMask = ((x.value ^ y.value) >> 31);

            if (nx >= ny)
            {
                int k = Atan2DivFast(ny.value, nx.value);
                int z = FPUtil.AtanPoly5Lut8(k);
                var angle = new fp32((negMask ^ (z >> 14)) - negMask);
                if (x.value > 0) return angle;
                if (y.value >= 0) return angle + PI;
                return angle - PI;
            }
            else
            {
                int k = Atan2DivFast(nx.value, ny.value);
                int z = FPUtil.AtanPoly5Lut8(k);
                var angle = new fp32(negMask ^ (z >> 14));
                return ((y.value > 0) ? PIHalf : -PIHalf) - angle;
            }
        }
        public static fp32 Atan2Fast2(fp32 y, fp32 x)
        {
            if (x.value == 0)
            {
                if (y.value > 0) return PIHalf;
                if (y.value < 0) return -PIHalf;

                throw new Exception("Fixed32.Atan2 out of range.");

            }

            var nx = Abs(x);
            var ny = Abs(y);
            int negMask = ((x.value ^ y.value) >> 31);

            if (nx >= ny)
            {
                int k = Atan2DivFast2(ny.value, nx.value);
                int z = FPUtil.AtanPoly5Lut8(k);
                var angle = new fp32((negMask ^ (z >> 14)) - negMask);
                if (x.value > 0) return angle;
                if (y.value >= 0) return angle + PI;
                return angle - PI;
            }
            else
            {
                int k = Atan2DivFast2(nx.value, ny.value);
                int z = FPUtil.AtanPoly5Lut8(k);
                var angle = new fp32(negMask ^ (z >> 14));
                return ((y.value > 0) ? PIHalf : -PIHalf) - angle;
            }
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int UnitSin(int z)
        {
            // See: http://www.coranac.com/2009/07/sines/

            // Handle quadrants 1 and 2 by mirroring the [1, 3] range to [-1, 1] (by calculating 2 - z).
            // The if condition uses the fact that for the quadrants of interest are 0b01 and 0b10 (top two bits are different).
            if ((z ^ (z << 1)) < 0)
                z = (1 << 31) - z;

            // Now z is in range [-1, 1].
            const int ONE = (1 << 30);
            Debug.Assert((z >= -ONE) && (z <= ONE));

            // Polynomial approximation.
            int zz = FPUtil.Qmul30(z, z);
            int res = FPUtil.Qmul30(FPUtil.SinPoly4(zz), z);

            // Return as s2.30.
            return res;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int UnitSinFast(int z)
        {
            // See: http://www.coranac.com/2009/07/sines/

            // Handle quadrants 1 and 2 by mirroring the [1, 3] range to [-1, 1] (by calculating 2 - z).
            // The if condition uses the fact that for the quadrants of interest are 0b01 and 0b10 (top two bits are different).
            if ((z ^ (z << 1)) < 0)
                z = (1 << 31) - z;

            // Now z is in range [-1, 1].
            const int ONE = (1 << 30);
            Debug.Assert((z >= -ONE) && (z <= ONE));

            // Polynomial approximation.
            int zz = FPUtil.Qmul30(z, z);
            int res = FPUtil.Qmul30(FPUtil.SinPoly3(zz), z);

            // Return as s2.30.
            return res;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int UnitSinFast2(int z)
        {
            // See: http://www.coranac.com/2009/07/sines/

            // Handle quadrants 1 and 2 by mirroring the [1, 3] range to [-1, 1] (by calculating 2 - z).
            // The if condition uses the fact that for the quadrants of interest are 0b01 and 0b10 (top two bits are different).
            if ((z ^ (z << 1)) < 0)
                z = (1 << 31) - z;

            // Now z is in range [-1, 1].
            const int ONE = (1 << 30);
            Debug.Assert((z >= -ONE) && (z <= ONE));

            // Polynomial approximation.
            int zz = FPUtil.Qmul30(z, z);
            int res = FPUtil.Qmul30(FPUtil.SinPoly2(zz), z);

            // Return as s2.30.
            return res;
        }


        public static fp32 Tan(fp32 x)
        {
            var z = new fp32(RCP_TWO_PI) * x;
            int sinX = UnitSin(z.value);
            int cosX = UnitSin(z.value + (1 << 30));
            return new fp32(sinX) / new fp32(cosX);
        }

        public static fp32 TanFast(fp32 x)
        {
            var z = new fp32(RCP_TWO_PI) * x;
            int sinX = UnitSinFast(z.value);
            int cosX = UnitSinFast(z.value + (1 << 30));
            return new fp32(sinX) / new fp32(cosX);
        }

        public static fp32 TanFastest(fp32 x)
        {
            var z = new fp32(RCP_TWO_PI) * x;
            int sinX = UnitSinFast2(z.value);
            int cosX = UnitSinFast2(z.value + (1 << 30));
            return new fp32(sinX) / new fp32(cosX);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 Atan(fp32 x)
        {
            return Atan2(x, one);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 AtanFast(fp32 x)
        {
            return Atan2Fast(x, one);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static fp32 AtanFast2(fp32 x)
        {
            return Atan2Fast2(x, one);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void SinCos(fp32 x, out fp32 s, out fp32 c)
        {
            s = Sin(x);
            c = Cos(x);
        }
        #endregion

        #region 插值
        public static fp32 Lerp(fp32 a, fp32 b, fp32 weight)
        {
            return (one - weight) * a + weight * b;
        }
        #endregion
    }
}
